/*! 
 * Implementation file for the rigid body class.
 *
 * \author Devis Rossini
 * \date 17 March 2012
 */ 
#include "PhxRigidBody.h"

#include <memory>
#include <cassert>

using namespace phx;

static inline void TransformInertiaTensor(Matrix3& iitWorld, const Quaternion& q, const Matrix3& iitBody, const Matrix4& rotmat)
{
    real t4 =	rotmat.data[0] * iitBody.data[0] +
				rotmat.data[1] * iitBody.data[3] +
				rotmat.data[2] * iitBody.data[6];

    real t9 =	rotmat.data[0] * iitBody.data[1] +
				rotmat.data[1] * iitBody.data[4] +
				rotmat.data[2] * iitBody.data[7];

    real t14 =	rotmat.data[0] * iitBody.data[2] +
				rotmat.data[1] * iitBody.data[5] + 
				rotmat.data[2] * iitBody.data[8];

    real t28 =	rotmat.data[4] * iitBody.data[0] +
				rotmat.data[5] * iitBody.data[3] +
				rotmat.data[6] * iitBody.data[6];

    real t33 =	rotmat.data[4] * iitBody.data[1] +
				rotmat.data[5] * iitBody.data[4] +
				rotmat.data[6] * iitBody.data[7];

    real t38 =	rotmat.data[4] * iitBody.data[2] +
				rotmat.data[5] * iitBody.data[5] +
				rotmat.data[6] * iitBody.data[8];

    real t52 =	rotmat.data[8] * iitBody.data[0] +
				rotmat.data[9] * iitBody.data[3] + 
				rotmat.data[10] * iitBody.data[6];

    real t57 =	rotmat.data[8] * iitBody.data[1]+
				rotmat.data[9] * iitBody.data[4]+
				rotmat.data[10] * iitBody.data[7];
		
    real t62 =	rotmat.data[8] * iitBody.data[2]+
				rotmat.data[9] * iitBody.data[5]+
				rotmat.data[10] * iitBody.data[8];

    iitWorld.data[0] =	t4 * rotmat.data[0] +
						t9 * rotmat.data[1] +
						t14 * rotmat.data[2];

    iitWorld.data[1] =	t4 * rotmat.data[4]+
						t9 * rotmat.data[5]+
						t14 * rotmat.data[6];

    iitWorld.data[2] =	t4 * rotmat.data[8] +
						t9 * rotmat.data[9] +
						t14 * rotmat.data[10];

    iitWorld.data[3] =	t28 * rotmat.data[0] +
						t33 * rotmat.data[1] +
						t38 * rotmat.data[2];

    iitWorld.data[4] =	t28 * rotmat.data[4] +
						t33 * rotmat.data[5] +
						t38 * rotmat.data[6];

    iitWorld.data[5] =	t28 * rotmat.data[8] +
						t33 * rotmat.data[9] +
						t38 * rotmat.data[10];

    iitWorld.data[6] =	t52 * rotmat.data[0] +
						t57 * rotmat.data[1] +
						t62 * rotmat.data[2];

    iitWorld.data[7] =	t52 * rotmat.data[4] +
						t57 * rotmat.data[5] +
						t62 * rotmat.data[6];

    iitWorld.data[8] =	t52 * rotmat.data[8] +
						t57 * rotmat.data[9] +
						t62 * rotmat.data[10];
}

static inline void CalculateTransformMatrix(Matrix4& transformMatrix, const Vector3& position, const Quaternion& orientation)
{
    transformMatrix.data[0] = 1 -	2 * orientation.j * orientation.j -
									2 * orientation.k * orientation.k;

    transformMatrix.data[1] =		2 * orientation.i * orientation.j -
									2 * orientation.r * orientation.k;

    transformMatrix.data[2] =		2 * orientation.i * orientation.k +
									2 * orientation.r * orientation.j;
    
	transformMatrix.data[3] =		position.x;

    transformMatrix.data[4] =		2 * orientation.i * orientation.j +
									2 * orientation.r * orientation.k;

    transformMatrix.data[5] = 1 -	2 * orientation.i * orientation.i -
									2 * orientation.k * orientation.k;

    transformMatrix.data[6] =		2 * orientation.j * orientation.k -
									2 * orientation.r * orientation.i;

    transformMatrix.data[7] =		position.y;

    transformMatrix.data[8] =		2 * orientation.i * orientation.k -
									2 * orientation.r * orientation.j;

    transformMatrix.data[9] =		2 * orientation.j * orientation.k +
									2 * orientation.r * orientation.i;

    transformMatrix.data[10] = 1 -	2 * orientation.i * orientation.i -
									2 * orientation.j * orientation.j;

    transformMatrix.data[11] =		position.z;
}

RigidBody::RigidBody() { }

RigidBody::RigidBody(const RigidBodyDescriptor& desc)
{
	SetupRigidBody(desc);
}

void RigidBody::SetupRigidBody(const RigidBodyDescriptor& desc)
{
	real inverseMass = 0;

	if (desc.mMass != 0)
		inverseMass = ((real)1.0) / desc.mMass;
		
	SetInverseMass(inverseMass);

	SetInertiaTensor(desc.mInertiaTensor);
	SetDamping(desc.mLinearDamping, desc.mAngularDamping);

	SetPosition(desc.mPosition);
	SetOrientation(desc.mOrientation);
	SetVelocity(desc.mVelocity);
	SetAngularVelocity(desc.mAngularVelocity);
	SetAcceleration(desc.mAcceleration);

	SetCanSleep(desc.mCanSleep);
	SetAwake(desc.mIsAwake);

	ClearAccumulators();
	CalculateDerivedData();
}

RigidBodyTypes RigidBody::GetBodyType() const
{
	return mBodyType;
}

bool RigidBody::IsStatic() const
{
	return (mBodyType == STATIC_BODY);
}

bool RigidBody::IsDynamic() const
{
	return (mBodyType == DYNAMIC_BODY);
}

void RigidBody::SetStatic()
{
	SetInverseMass(0);
}

void RigidBody::SetDynamic(real mass)
{
	SetMass(mass);
}

void RigidBody::CalculateDerivedData()
{
    mOrientation.Normalise();

    // Calculate the transform matrix for the body.
    CalculateTransformMatrix( mTransformMatrix, mPosition, mOrientation );

    // Calculate the inertia tensor in world space.
    TransformInertiaTensor(	mInverseInertiaTensorWorld, 
							mOrientation,
							mInverseInertiaTensor,
							mTransformMatrix );
}

void RigidBody::Integrate(real duration)
{
	if (!HasFiniteMass()) return;
    if (!mIsAwake) return;

    // Calculate linear acceleration from force inputs.
    mLastFrameAcceleration = mAcceleration;
    mLastFrameAcceleration.AddScaledVector(mForceAccumulator, mInverseMass);

    // Calculate angular acceleration from torque inputs.
    Vector3 angularAcceleration =
        mInverseInertiaTensorWorld.Transform(mTorqueAccumulator);

    // Update linear velocity from both acceleration and impulse.
    mVelocity.AddScaledVector(mLastFrameAcceleration, duration);

    // Update angular velocity from both acceleration and impulse.
    mAngularVelocity.AddScaledVector(angularAcceleration, duration);

    // Impose drag.
    mVelocity			*= real_pow(mLinearDamping,		duration);
    mAngularVelocity	*= real_pow(mAngularDamping,	duration);

    // Update linear position.
    mPosition.AddScaledVector(mVelocity, duration);

    // Update angular position.
    mOrientation.AddScaledVector(mAngularVelocity, duration);

    // Normalise the orientation, and update the matrices with the new
    // position and orientation
    CalculateDerivedData();

    // Clear accumulators.
    ClearAccumulators();

    // Update the kinetic energy store, and possibly put the body to
    // sleep.
    if (mCanSleep) 
	{
        real currentMotion =	mVelocity.DotProduct(mVelocity) +
								mAngularVelocity.DotProduct(mAngularVelocity);

		real bias = real_pow(0.5, duration);
		mMotion = bias * mMotion + (1 - bias) * currentMotion;

		if (mMotion < sleepEpsilon) SetAwake(false);
		else if (mMotion > 10 * sleepEpsilon) mMotion = 10 * sleepEpsilon;
	}
}

void RigidBody::SetMass(const real mass)
{
	assert(mass != 0);
    mInverseMass = ((real)1.0) / mass;

	mBodyType = DYNAMIC_BODY;

	// Set mass and body type.
	//if (mass != 0)
	//{
	//	mInverseMass = ((real)1.0 / mass);
	//	mBodyType = DYNAMIC_BODY;
	//}
	//else 
	//{
	//	mInverseMass = 0;
	//	mBodyType = STATIC_BODY;
	//}
}

real RigidBody::GetMass() const
{
    if (mInverseMass == 0) 
	{
        return REAL_MAX;
    } 
	else 
	{
        return ((real)1.0) / mInverseMass;
    }
}

void RigidBody::SetInverseMass(const real inverseMass)
{
	mInverseMass = inverseMass;

	// Set body type.
	mBodyType = inverseMass != 0 ? DYNAMIC_BODY : STATIC_BODY; 
}

real RigidBody::GetInverseMass() const
{
    return mInverseMass;
}

bool RigidBody::HasFiniteMass() const
{
    return mInverseMass > 0.0f;
}

void RigidBody::SetInertiaTensor(const Matrix3& inertiaTensor)
{
    mInverseInertiaTensor.SetInverse(inertiaTensor);
}

void RigidBody::GetInertiaTensor(Matrix3* inertiaTensor) const
{
    inertiaTensor->SetInverse(mInverseInertiaTensor);
}

Matrix3 RigidBody::GetInertiaTensor() const
{
    Matrix3 it;
    GetInertiaTensor(&it);
    return it;
}

void RigidBody::GetInertiaTensorWorld(Matrix3* inertiaTensor) const
{
    inertiaTensor->SetInverse(mInverseInertiaTensorWorld);
}

Matrix3 RigidBody::GetInertiaTensorWorld() const
{
    Matrix3 it;
    GetInertiaTensorWorld(&it);
    return it;
}

void RigidBody::SetInverseInertiaTensor(const Matrix3& inverseInertiaTensor)
{
	mInverseInertiaTensor = inverseInertiaTensor;
}

void RigidBody::GetInverseInertiaTensor(Matrix3* inverseInertiaTensor) const
{
    *inverseInertiaTensor = mInverseInertiaTensor;
}

Matrix3 RigidBody::GetInverseInertiaTensor() const
{
    return mInverseInertiaTensor;
}

void RigidBody::GetInverseInertiaTensorWorld(Matrix3* inverseInertiaTensor) const
{
    *inverseInertiaTensor = mInverseInertiaTensorWorld;
}

Matrix3 RigidBody::GetInverseInertiaTensorWorld() const
{
    return mInverseInertiaTensorWorld;
}

void RigidBody::SetDamping(const real linearDamping, const real angularDamping)
{
    mLinearDamping	= linearDamping;
    mAngularDamping = angularDamping;
}

void RigidBody::SetLinearDamping(const real linearDamping)
{
	mLinearDamping = linearDamping;
}

real RigidBody::GetLinearDamping() const
{
    return mLinearDamping;
}

void RigidBody::SetAngularDamping(const real angularDamping)
{
    mAngularDamping = angularDamping;
}

real RigidBody::GetAngularDamping() const
{
    return mAngularDamping;
}

void RigidBody::SetPosition(const Vector3& position)
{
    mPosition = position;
}

void RigidBody::SetPosition(const real x, const real y, const real z)
{
    mPosition.x = x;
    mPosition.y = y;
    mPosition.z = z;
}

void RigidBody::GetPosition(Vector3* position) const
{
    *position = mPosition;
}

Vector3 RigidBody::GetPosition() const
{
    return mPosition;
}

void RigidBody::SetOrientation(const Quaternion& orientation)
{
    mOrientation = orientation;
    mOrientation.Normalise();
}

void RigidBody::SetOrientation(const real r, const real i, const real j, const real k)
{
    mOrientation.r = r;
    mOrientation.i = i;
    mOrientation.j = j;
    mOrientation.k = k;
    mOrientation.Normalise();
}

void RigidBody::GetOrientation(Quaternion* orientation) const
{
    *orientation = mOrientation;
}

Quaternion RigidBody::GetOrientation() const
{
    return mOrientation;
}

void RigidBody::GetOrientation(Matrix3* matrix) const
{
	GetOrientation(matrix->data);
}

void RigidBody::GetOrientation(real matrix[9]) const
{
    matrix[0] = mTransformMatrix.data[0];
    matrix[1] = mTransformMatrix.data[1];
    matrix[2] = mTransformMatrix.data[2];

    matrix[3] = mTransformMatrix.data[4];
    matrix[4] = mTransformMatrix.data[5];
    matrix[5] = mTransformMatrix.data[6];

    matrix[6] = mTransformMatrix.data[8];
    matrix[7] = mTransformMatrix.data[9];
    matrix[8] = mTransformMatrix.data[10];
}

void RigidBody::GetTransform(Matrix4* transform) const
{
    memcpy(transform, &mTransformMatrix.data, sizeof(Matrix4));
}

void RigidBody::GetTransform(real matrix[16]) const
{
    memcpy(matrix, mTransformMatrix.data, sizeof(real) * 12);
    matrix[12] = matrix[13] = matrix[14] = 0;
    matrix[15] = 1;
}

Matrix4 RigidBody::GetTransform() const
{
	return mTransformMatrix;
}

Vector3 RigidBody::GetPointInLocalSpace(const Vector3& point) const
{
    return mTransformMatrix.TransformInverse(point);
}

Vector3 RigidBody::GetPointInWorldSpace(const Vector3& point) const
{
    return mTransformMatrix.Transform(point);
}

Vector3 RigidBody::GetDirectionInLocalSpace(const Vector3& direction) const
{
    return mTransformMatrix.TransformInverseDirection(direction);
}

Vector3 RigidBody::GetDirectionInWorldSpace(const Vector3& direction) const
{
    return mTransformMatrix.TransformDirection(direction);
}

void RigidBody::SetVelocity(const Vector3& velocity)
{
    mVelocity = velocity;
}

void RigidBody::SetVelocity(const real x, const real y, const real z)
{
    mVelocity.x = x;
    mVelocity.y = y;
    mVelocity.z = z;
}

void RigidBody::GetVelocity(Vector3* velocity) const
{
    *velocity = mVelocity;
}

Vector3 RigidBody::GetVelocity() const
{
    return mVelocity;
}

void RigidBody::AddVelocity(const Vector3& deltaVelocity)
{
    mVelocity += deltaVelocity;
}

void RigidBody::SetAngularVelocity(const Vector3& angularVelocity)
{
	mAngularVelocity = angularVelocity;
}

void RigidBody::SetAngularVelocity(const real x, const real y, const real z)
{
    mAngularVelocity.x = x;
    mAngularVelocity.y = y;
    mAngularVelocity.z = z;
}

void RigidBody::GetAngularVelocity(Vector3* angularVelocity) const
{
    *angularVelocity = mAngularVelocity;
}

Vector3 RigidBody::GetAngularVelocity() const
{
    return mAngularVelocity;
}

void RigidBody::AddAngularVelocity(const Vector3& deltaAngularVelocity)
{
    mAngularVelocity += deltaAngularVelocity;
}

void RigidBody::SetAwake(const bool awake)
{
    if (awake) 
	{
		mIsAwake = true;

        // Add a bit of motion to avoid it falling asleep immediately.
        mMotion = sleepEpsilon * 2.0f;
    } 
	else 
	{
        mIsAwake = false;
        mVelocity.Clear();
        mAngularVelocity.Clear();
    }
}

void RigidBody::SetCanSleep(const bool canSleep)
{
    mCanSleep = canSleep;

    if (!mCanSleep && !mIsAwake) SetAwake();
}

void RigidBody::GetLastFrameAcceleration(Vector3* acceleration) const
{
    *acceleration = mLastFrameAcceleration;
}

Vector3 RigidBody::GetLastFrameAcceleration() const
{
    return mLastFrameAcceleration;
}

void RigidBody::ClearAccumulators()
{
    mForceAccumulator.Clear();
    mTorqueAccumulator.Clear();
}

void RigidBody::AddForce(const Vector3& force)
{
    mForceAccumulator += force;
    mIsAwake = true;
}

void RigidBody::AddForceAtBodyPoint(const Vector3& force, const Vector3& point)
{
    Vector3 pt = GetPointInWorldSpace(point);
    AddForceAtPoint(force, pt);

    mIsAwake = true;
}

void RigidBody::AddForceAtPoint(const Vector3& force, const Vector3& point)
{
    Vector3 pt = point;
    pt -= mPosition;

    mForceAccumulator	+= force;
    mTorqueAccumulator	+= pt % force; // It's a cross product ^_^

    mIsAwake = true;
}

void RigidBody::AddTorque(const Vector3 &torque)
{
    mTorqueAccumulator += torque;
    mIsAwake = true;
}

void RigidBody::SetAcceleration(const Vector3& acceleration)
{
	mAcceleration = acceleration;
}

void RigidBody::SetAcceleration(const real x, const real y, const real z)
{
    mAcceleration.x = x;
    mAcceleration.y = y;
    mAcceleration.z = z;
}

void RigidBody::GetAcceleration(Vector3* acceleration) const
{
    *acceleration = mAcceleration;
}

Vector3 RigidBody::GetAcceleration() const
{
    return mAcceleration;
}